import numpy as np
import cv2
import math
from scipy.spatial.transform import Rotation as R


class CameraInfo():
    """ Camera intrisics for point cloud creation. """

    def __init__(self, width, height, fx, fy, cx, cy, z_distance):
        self.width = width
        self.height = height
        self.fx = fx
        self.fy = fy
        self.cx = cx
        self.cy = cy
        self.z_distance = z_distance


def get_angle_width(box, cx, cy, w_pixel, depth, intrinsic):
    point_left_bottom = box[0]
    point_left_top = box[1]
    point_right_top = box[2]
    point_right_bottom = box[3]
    horizontal_slope = 0
    INF = 50000

    distance_left = np.array(point_left_top) - np.array(point_left_bottom)
    distance_right = np.array(point_right_top) - np.array(point_left_top)

    line_left = math.hypot(distance_left[0], distance_left[1])
    line_right = math.hypot(distance_right[0], distance_right[1])

    left_y_dis = point_left_top[1] - point_left_bottom[1]
    left_x_dis = point_left_top[0] - point_left_bottom[0]
    right_y_dis = point_right_top[1] - point_left_top[1]
    right_x_dis = point_right_top[0] - point_left_top[0]

    if line_left >= line_right:
        # get angle
        if left_x_dis != 0:
            line_slope = - (left_y_dis) / (left_x_dis)
        else:
            line_slope = INF
        angle = math.atan(abs((horizontal_slope - line_slope) / (1 + horizontal_slope * line_slope)))
        angle = 180 - math.degrees(angle)

        # get width
        complement_angle = 90 - angle
        dh = w_pixel / 2 * np.sin(complement_angle / 180)
        dw = w_pixel / 2 * np.cos(complement_angle / 180)
        cl = (cx - dw, cy - dh)
        cr = (cx + dw, cy + dh)
        width = math.sqrt(
            ((cr[0] - cl[0]) * depth / intrinsic.fx) ** 2 + ((cl[1] - cl[1]) * depth / intrinsic.fy) ** 2)
    else:
        # line_left < line_right
        # get angle
        if right_x_dis != 0:
            line_slope = - (right_y_dis) / (right_x_dis)
        else:
            line_slope = INF
        angle = math.atan(abs((horizontal_slope - line_slope) / (1 + horizontal_slope * line_slope)))
        angle = math.degrees(angle)

        # get width
        complement_angle = angle - 90
        dh = w_pixel / 2 * np.sin(complement_angle / 180)
        dw = w_pixel / 2 * np.cos(complement_angle / 180)
        # 左低右高
        cl = (cx - dw, cy + dh)
        cr = (cx + dw, cy - dh)
        width = math.sqrt(
            ((cr[0] - cl[0]) * depth / intrinsic.fx) ** 2 + ((cl[1] - cl[1]) * depth / intrinsic.fy) ** 2)

    return angle, width


def viewpoint_params_to_matrix(towards, angle):
    """
    **Input:**

    - towards: numpy array of shape (3,) of the toward vector

    - angle: float of the inplane rotation angle

    **Output:**

    - numpy array of shape (3,3) of the output rotation matrix
    """
    axis_x = towards  # 朝向依旧是x,y,z
    axis_y = np.array([-axis_x[1], axis_x[0], 0])
    if np.linalg.norm(axis_y) == 0:
        axis_y = np.array([0, 1, 0])
    axis_x = axis_x / np.linalg.norm(axis_x)
    axis_y = axis_y / np.linalg.norm(axis_y)
    axis_z = np.cross(axis_x, axis_y)
    # 绕x轴旋转
    R1 = np.array(
        [
            [1, 0, 0],
            [0, np.cos(angle), -np.sin(angle)],
            [0, np.sin(angle), np.cos(angle)],
        ]
    )

    # 绕y轴旋转
    """
        R1 = np.array(
        [
            [np.cos(angle), 0, np.sin(angle)],
            [0, 1, 0],
            [-np.sin(angle), 0, np.cos(angle)],
        ]
    )
    """
    # 绕z轴旋转
    """
            R1 = np.array(
        [
            [np.cos(angle), np.sin(angle), 0],
            [-np.sin(angle), np.cos(angle), 0],
            [0, 0, 1],
        ]
    )
    """
    R2 = np.c_[axis_x, np.c_[axis_y, axis_z]]
    matrix = R2.dot(R1)
    return matrix


def grasp_convert_4X4(translation, rotation_matrix):
    message = ""
    # concat gg_T & gg_R -> 4 x 4
    translations = np.append(translation, [1]).reshape(4, 1)
    zeros = np.array([0, 0, 0]).reshape(1, 3)
    rotations = np.concatenate((rotation_matrix, zeros), axis=0)
    # print(rotations)
    gg_in_camera = np.concatenate((rotations, translations), axis=1)
    print('gg_in_camera')
    print(f"\n{gg_in_camera}\n")

    # covert matrix
    # camera_2_robot = np.array([-0.4487174, -0.8883020, -0.0978376, 0.63352305,
    #                            -0.8823627, 0.4577381, -0.1091421, 0.74894029,
    #                            0.1417352, 0.0373543, -0.9891996, 1.6914822,
    #                            0, 0, 0, 1]).reshape((4, 4))

    camera_2_robot = np.array([-0.9648097111560694, -0.2601260910489137, 0.03842704795525438, 0.2046356977200156,
                               0.01755456649399863, 0.08209320997531433, 0.9964700407293524, -0.1814732502548702,
                               -0.2623624562588004, 0.9620785423402612, -0.07463792544201608, 1.017693156230031,
                               0, 0, 0, 1]).reshape((4, 4))

    print('camera_2_robot')
    print(f"\n{camera_2_robot}\n")
    # gg_in_robot = np.matmul(camera_2_robot, gg_in_camera)
    # gg_in_robot = np.invert(camera_2_robot).dot(gg_in_camera)
    gg_in_robot = np.dot(np.linalg.inv(camera_2_robot), gg_in_camera)
    # print(gg_in_robot_test)
    print('gg_in_robot')
    print(f"\n{gg_in_robot}\n")
    gg_in_robot = gg_in_robot.flatten('C')
    for num in gg_in_robot:
        message += f"{num} "
    message = f"\n\n{message}\n\n"

    return message


def rotation_matrix_to_euler_angles(R):
    """
    Convert a rotation matrix to Euler angles in Z-Y-X order.

    Parameters:
    R (numpy.ndarray): 3x3 rotation matrix

    Returns:
    numpy.ndarray: Array of Euler angles [roll, pitch, yaw]
    """
    assert (R.shape == (3, 3))

    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])


def grasp_convert_xyzrpy(translation, rotation_matrix):
    # message = ""
    # concat gg_T & gg_R -> 4 x 4
    translations = np.append(translation, [1]).reshape(4, 1)
    zeros = np.array([0, 0, 0]).reshape(1, 3)
    rotations = np.concatenate((rotation_matrix, zeros), axis=0)
    # print(rotations)
    gg_in_camera = np.concatenate((rotations, translations), axis=1)
    # print('gg_in_camera')
    # print(f"\n{gg_in_camera}\n")

    camera_2_robot = np.array([1, 0, 0, 0.11074058066098337,
                               0, 1, 0, 0.1200564639157477,
                               0, 0, -1, 1.034987007801811,
                               0, 0, 0, 1]).reshape((4, 4))

    # print('camera_2_robot')
    # print(f"\n{camera_2_robot}\n")
    # gg_in_robot = np.matmul(camera_2_robot, gg_in_camera)
    # gg_in_robot = np.dot(np.linalg.inv(camera_2_robot), gg_in_camera)
    gg_in_robot = np.dot(camera_2_robot, gg_in_camera)

    # 3DoF m | aix
    translation_vector = gg_in_robot[:3, 3]
    gg_x = translation_vector[1]
    gg_y = translation_vector[0]
    gg_z = translation_vector[2] + 0.01
    gg_rx = 2.887
    gg_ry = -1.196
    gg_rz = -0.01
    grasp_pose = [gg_x, gg_y, gg_z, gg_rx, gg_ry, gg_rz]
    # message = '\n\n' + ' '.join(map(str, grasp_pose)) + '\n\n'
    print("X Y Z R P Y: \n", grasp_pose)

    return grasp_pose
